#include "pointcloud_detect.h"
#include <opencv2/ximgproc.hpp>

/*
 * 雷达点云圆弧检测与匹配流程
 * ==========================
 * 
 * 1. 点云数据处理
 *    - 从ROS消息获取点云数据
 *    - 对z坐标进行归零处理
 *    - 选择指定范围的点云数据进行处理
 *    - 将点云数据映射到2D图像平面，生成二值图像
 * 
 * 2. 图像预处理
 *    - 将点云图像进行二值化处理
 *    - 对二值图像进行膨胀操作，填充细小间隙
 *    - 使用Zhang-Suen细化算法提取骨架，形成连续线条
 * 
 * 3. 轮廓提取与筛选
 *    - 使用OpenCV的findContours提取所有轮廓
 *    - 筛选轮廓，排除点数小于5的轮廓(椭圆拟合需要至少5个点)
 *    - 基于轮廓长度(MIN_CONTOUR_LENGTH)进行筛选
 *    - 基于长宽比(1.0-1.8之间)进行筛选
 * 
 * 4. 椭圆拟合与圆弧检测
 *    - 对筛选后的轮廓使用cv::fitEllipse进行椭圆拟合
 *    - 计算圆弧半径(椭圆长短轴的平均值)
 *    - 计算圆弧角度范围(通过角度排序和间隙分析)
 *    - 计算每个圆弧的置信度评分
 * 
 * 5. 评分与筛选
 *    - 基于半径评分：与目标半径(TARGET_RADIUS)的接近程度
 *    - 基于角度评分：与目标角度范围(MIN_ARC_ANGLE-MAX_ARC_ANGLE)的匹配程度
 *    - 基于径向分布评分：评估点到圆心距离的均匀性
 *    - 综合以上评分，使用权重计算总置信度
 *    - 筛选置信度大于阈值(CONFIDENCE_THRESHOLD)的圆弧
 * 
 * 6. 结果处理与可视化
 *    - 按置信度排序，选择最佳圆弧
 *    - 保存上一次有效结果，允许在当前帧未检测到时复用
 *    - 将图像坐标转换回点云坐标
 *    - 对圆弧中心点进行低通滤波平滑处理，减少抖动
 *    - 可视化结果：绘制轮廓、中心点、半径信息等
 *    - 通过ROS话题发布处理后的点云和中心点数据
 */

PointCloudDetect::PointCloudDetect() : rng(std::random_device()()) {
    // 初始化ROS节点
    ros::NodeHandle nh;
    cross_pub = nh.advertise<sensor_msgs::PointCloud2>("cross_point", 10);
    keypoint_pub = nh.advertise<sensor_msgs::PointCloud2>("keypoint_cloud", 10);
    origincloud_pub = nh.advertise<sensor_msgs::PointCloud2>("origin_cloud", 10);
    halfcloud_pub = nh.advertise<sensor_msgs::PointCloud2>("half_cloud", 10);  // 新增：发布前半部分点云
    
        // 找到点云的边界
    min_x = std::numeric_limits<float>::max();
    max_x = std::numeric_limits<float>::min();
    min_y = std::numeric_limits<float>::max();
    max_y = std::numeric_limits<float>::min();
    
    // 初始化点云范围
    cloud_start_idx = 0;
    cloud_end_idx = 0; // 0表示使用到最后一个点
    
    // 初始化轮廓记忆
    has_valid_segment = false;
    use_last_arc_count = 0;
    
    // 初始化帧计数
    frame_count = 0;
    
    // ROS_INFO("点云处理初始化完成，开始记录总帧数");
}

PointCloudDetect::~PointCloudDetect() {}

// 通用的发布点云函数
void PointCloudDetect::publishPointCloud(const PointCloud2DPtr& cloud, ros::Publisher& pub)
{
    sensor_msgs::PointCloud2 output;
    pointCloud2DToRosMsg(*cloud, output);
    output.header.frame_id = "map"; // 设置frame_id
    output.header.stamp = ros::Time::now(); // 设置时间戳
    pub.publish(output);
}

void PointCloudDetect::showImage(const cv::Mat& image, const std::string& window_name)
{
    cv::imshow(window_name, image);
    cv::waitKey(1);  // 等待1毫秒，以更新图像窗口
}

void PointCloudDetect::rosMsgToPointCloud2D(const sensor_msgs::PointCloud2& cloud_msg, PointCloud2D& cloud)
{
    cloud.resize(cloud_msg.data.size() / cloud_msg.point_step);
    for (size_t i = 0; i < cloud.size(); ++i) {
        const uint8_t* data = &cloud_msg.data[i * cloud_msg.point_step];
        cloud[i].x = *reinterpret_cast<const float*>(data + 0);
        cloud[i].y = *reinterpret_cast<const float*>(data + 4);
        // cloud[i].yaw = *reinterpret_cast<const float*>(data + 8); // 假设yaw字段在data的第8个字节开始
    }
}

// 函数用于将PointCloud2D转换为ROS的点云消息
void PointCloudDetect::pointCloud2DToRosMsg(const PointCloud2D& cloud, sensor_msgs::PointCloud2& cloud_msg)
{
    // 设置点云消息的基本信息
    cloud_msg.header.frame_id = "map"; // 根据实际情况设置frame_id
    cloud_msg.height = 1;
    cloud_msg.width = cloud.size();
    cloud_msg.is_dense = false;
    cloud_msg.point_step = 16; // 每个点有4个float字段：x, y, z, intensity
    cloud_msg.row_step = cloud_msg.point_step * cloud_msg.width;
    cloud_msg.fields.resize(4);

    // 设置字段信息
    cloud_msg.fields[0].name = "x";
    cloud_msg.fields[0].offset = 0;
    cloud_msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
    cloud_msg.fields[0].count = 1;

    cloud_msg.fields[1].name = "y";
    cloud_msg.fields[1].offset = 4;
    cloud_msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
    cloud_msg.fields[1].count = 1;

    cloud_msg.fields[2].name = "z";
    cloud_msg.fields[2].offset = 8;
    cloud_msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
    cloud_msg.fields[2].count = 1;

    cloud_msg.fields[3].name = "intensity";
    cloud_msg.fields[3].offset = 12;
    cloud_msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
    cloud_msg.fields[3].count = 1;

    // 分配数据空间
    cloud_msg.data.resize(cloud_msg.row_step);

    // 填充数据
    for (size_t i = 0; i < cloud.size(); ++i) {
        uint8_t* data = &cloud_msg.data[i * cloud_msg.point_step];
        *reinterpret_cast<float*>(data + 0) = cloud[i].x;
        *reinterpret_cast<float*>(data + 4) = cloud[i].y;
        *reinterpret_cast<float*>(data + 8) = 0; // z设置为0
        *reinterpret_cast<float*>(data + 12) = 1.0; // 设置intensity为1.0
    }
}

// 函数用于将PointCloud2DPtr转换为图像
cv::Mat PointCloudDetect::pointCloud2DToImage(const PointCloud2DPtr& cloud)
{
    if (cloud->empty())
    {
        return cv::Mat();
    }

    for (const auto& point : *cloud)
    {
        float scaled_x = point.x * 100;
        float scaled_y = point.y * 100;
        if (scaled_x < min_x) min_x = scaled_x;
        if (scaled_x > max_x) max_x = scaled_x;
        if (scaled_y < min_y) min_y = scaled_y;
        if (scaled_y > max_y) max_y = scaled_y;
    }
    // 打印边界信息
    // ROS_INFO("PointCloud2DToImage: min_x = %f, max_x = %f, min_y = %f, max_y = %f", min_x, max_x, min_y, max_y);

    // 计算图像的宽度和高度
    int width = static_cast<int>(max_x - min_x) + 1 + 2 * this->border;  // 两边各加10像素
    int height = static_cast<int>(max_y - min_y) + 1 + 2 * this->border; // 上下各加10像素

    // ROS_INFO("PointCloud2DToImage: width = %d, height = %d", width, height);
    // 创建一个彩色图像
    cv::Mat image(height, width, CV_8UC3, cv::Scalar(0, 0, 0));

    // 将点云数据映射到图像中
    for (const auto& point : *cloud)
    {
        int x = static_cast<int>(point.x * 100 - min_x) + this->border;
        int y = static_cast<int>(point.y * 100 - min_y) + this->border;
        if (x >= this->border && x < width - this->border && y >= this->border && y < height - this->border)
        {
            // 设置点的颜色，白色
            image.at<cv::Vec3b>(y, x) = cv::Vec3b(255, 255, 255);  
        }
    }
    return image;
}

cv::Point2f PointCloudDetect::imageToPointCloud(const cv::Point2f& image_point)
{
    // 逆坐标轴交换
    // 从图像坐标转换回点云坐标时，需要减去边框的偏移
    float x_cloud = (image_point.x - this->border + min_x) / 100.0f;
    float y_cloud = (image_point.y - this->border + min_y) / 100.0f;
    
    return cv::Point2f(x_cloud, y_cloud);
}

// 辅助函数 - 根据范围计算评分
float PointCloudDetect::calculateScoreFromRange(float value, float targetValue, float minValue, float maxValue) {
    return 1.0f - std::abs(value - targetValue) / (maxValue - minValue);
}

// 辅助函数 - 计算椭圆上的点坐标
cv::Point2f PointCloudDetect::calculateEllipsePoint(const cv::Point2f& center, float a, float b, float angle, float rotAngle) {
    float x = center.x + a * std::cos(angle + rotAngle) * std::cos(rotAngle) - 
                        b * std::sin(angle + rotAngle) * std::sin(rotAngle);
    float y = center.y + a * std::cos(angle + rotAngle) * std::sin(rotAngle) + 
                        b * std::sin(angle + rotAngle) * std::cos(rotAngle);
    return cv::Point2f(x, y);
}

// 图像预处理函数 - 提取连续的骨架轨迹
void PointCloudDetect::preprocessImage(const cv::Mat& input, cv::Mat& output) {
    // 创建灰度图
    cv::Mat gray;
    if (input.channels() == 3) {
        cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY);
    } else {
        input.copyTo(gray);  // 直接引用避免克隆
    }
    
    // 二值化 - 直接使用gray作为输入，输出到output
    cv::threshold(gray, output, 128, 255, cv::THRESH_BINARY);
    // 我们已经不显示中间结果，可以移除这些调用
    // showDebugImage(output, "1-二值化");
    // ROS_INFO("二值化图像非零像素数: %d", cv::countNonZero(output));
    
    // 膨胀处理 - 重用output
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(9, 9));
    cv::dilate(output, output, kernel);
    // showDebugImage(output, "2-膨胀结果");
    // ROS_INFO("膨胀后图像非零像素数: %d", cv::countNonZero(output));


    // 使用细化算法获得骨架 - 临时变量用于存储骨架
    cv::ximgproc::thinning(output, output, cv::ximgproc::THINNING_ZHANGSUEN);
    showDebugImage(output, "3-细化结果");

    // 对于output图像，连接0.02间距内的点，完善缺漏点

}

// 提取轮廓
std::vector<std::vector<cv::Point>> PointCloudDetect::extractContours(const cv::Mat& binary) {
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    
    cv::findContours(binary, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    
    return contours;
}

// 使用椭圆拟合计算半径
float PointCloudDetect::calculateRadius(const std::vector<cv::Point>& contour, const cv::RotatedRect& ellipse) {
    if (contour.size() < 5) {
        // 点数太少，使用平均距离计算
        float sumRadius = 0;
        const cv::Point2f& center = ellipse.center;
        for (const auto& point : contour) {
            float dx = point.x - center.x;
            float dy = point.y - center.y;
            sumRadius += std::sqrt(dx*dx + dy*dy);
        }
        return sumRadius / contour.size();
    }
    
    // 直接使用传入的椭圆参数
    // 椭圆的半长轴和半短轴平均值作为半径
    return (ellipse.size.width + ellipse.size.height) / 4.0f;
}

// 使用椭圆拟合计算角度范围
float PointCloudDetect::calculateArcAngle(const std::vector<cv::Point>& contour, const cv::RotatedRect& ellipse) {
    if (contour.size() < 5) return 0.0f; // 如果点太少，返回0
    
    // 获取椭圆中心点
    const cv::Point2f& center = ellipse.center;
    
    // 计算所有点相对于中心的角度
    std::vector<float> angles;
    angles.reserve(contour.size());
    
    for (const auto& point : contour) {
        float dx = point.x - center.x;
        float dy = point.y - center.y;
        float angle = std::atan2(dy, dx);
        if (angle < 0) angle += 2 * M_PI; // 确保所有角度在[0, 2π)范围内
        angles.push_back(angle);
    }
    
    // 对角度进行排序
    std::sort(angles.begin(), angles.end());
    
    // 优化寻找最大间隙的算法
    float maxGap = 0;
    size_t maxGapIdx = 0;
    for (size_t i = 0; i < angles.size() - 1; i++) {
        float gap = angles[i+1] - angles[i];
        if (gap > maxGap) {
            maxGap = gap;
            maxGapIdx = i;
        }
    }
    // 检查首尾间隙
    float endGap = angles[0] + 2 * M_PI - angles.back();
    if (endGap > maxGap) {
        maxGap = endGap;
    }
    
    // 圆弧角度 = 2π - 最大间隙
    float arcAngle = 2 * M_PI - maxGap;
    
    // 调试输出
    // ROS_INFO("角度计算: 最大间隙=%.1f度, 覆盖角度=%.1f度", 
            //  maxGap * 180.0f / M_PI, arcAngle * 180.0f / M_PI);
    
    return arcAngle;
}

// 添加基于点到中心距离分布的评分函数
float PointCloudDetect::calculateRadialDistribution(const std::vector<cv::Point>& contour, const cv::Point2f& center) {
    if (contour.size() < 3) return 0.0f;
    
    // 计算每个点到中心的距离
    std::vector<float> radialDistances;
    radialDistances.reserve(contour.size());
    
    for (const auto& point : contour) {
        float dist = cv::norm(cv::Point2f(point) - center);
        radialDistances.push_back(dist);
    }
    
    // 计算径向距离的均值和标准差
    float meanDist = std::accumulate(radialDistances.begin(), radialDistances.end(), 0.0f) / radialDistances.size();
    
    float variance = 0.0f;
    for (float dist : radialDistances) {
        variance += (dist - meanDist) * (dist - meanDist);
    }
    variance /= radialDistances.size();
    float stdDev = std::sqrt(variance);
    
    // 计算变异系数 (标准差/均值)
    // 对于完美圆弧，变异系数应接近0
    float cv = meanDist > 0 ? stdDev / meanDist : 1.0f;
    
    // 将变异系数映射到0-1的分数范围（变异系数越小，分数越高）
    // 使用指数衰减函数增强区分度
    float score = std::exp(-5.0f * cv);
    
    // 限制在0-1范围内
    return std::max(0.0f, std::min(1.0f, score));
}

// 图像处理函数
ArcSegment PointCloudDetect::processImageCircle(const cv::Mat& image)
{
    // 创建一个空的ArcSegment用于返回
    ArcSegment resultArc;
    resultArc.confidence = 0.0f; // 初始化置信度为0，表示未找到有效轮廓
    
    // 1. 检查是否需要使用上一次有效轮廓
    bool useLastArc = false;
    std::string errorReason = "";
    
    // 输入图像为空
    if (image.empty()) {
        errorReason = "输入图像为空";
        useLastArc = true;
    }
    showDebugImage(image, "1-输入图像");
    // 如果不使用上一次轮廓，继续处理
    if (!useLastArc) {
        // 创建结果图像（彩色），仅在需要时复制
        cv::Mat result;
        if (image.channels() == 3) {
            // 如果已经是彩色图像，可以直接使用
            result = image;
        } else {
            // 如果是灰度图像，转换为彩色
            cv::cvtColor(image, result, cv::COLOR_GRAY2BGR);
        }
        
        // 2. 预处理图像并提取轮廓
        std::vector<std::vector<cv::Point>> allContours;
        cv::Mat processed = preprocessAndExtractContours(image, allContours);
        
        // 检查轮廓数量
        if (allContours.empty()) {
            errorReason = "未检测到任何轮廓";
            useLastArc = true;
        }
        cv::Mat image_copy = image.clone();
        drawContours(image_copy, allContours, -1, cv::Scalar(0, 0, 255), 2);
        showDebugImage(image_copy, "2-预处理结果");
        // 3. 如果有轮廓，继续筛选
        if (!useLastArc) {
            std::vector<std::vector<cv::Point>> filteredContours = filterContours(allContours, image);
            
            // 检查筛选后的轮廓数量
            if (filteredContours.empty()) {
                errorReason = "没有符合条件的轮廓";
                useLastArc = true;
            }

            // 4. 如果有符合条件的轮廓，进行椭圆拟合
            if (!useLastArc) {
                std::vector<ArcSegment> detectedArcs = detectArcsFromContours(filteredContours, image);
                
                // 5. 如果检测到了有效的圆弧
                if (!detectedArcs.empty()) {
                    // 找到了有效的轮廓，重置计数器
                    use_last_arc_count = 0;
                    
                    // 返回检测到的最佳轮廓
                    resultArc = detectedArcs[0];
                    // resultArc.ellipse.center = getLowPassSmoothCenter(resultArc.ellipse.center);
                    visualizeAndPublishResults(resultArc, result);
                    
                    // 保存此轮廓为最后一个有效轮廓
                    last_valid_arc = resultArc;
                    has_valid_segment = true;
                } else {
                    errorReason = "未检测到任何符合条件的圆弧";
                    useLastArc = true;
                }
            }
        }
    }
    
    // 处理是否使用上一次有效轮廓的逻辑
    if (useLastArc && has_valid_segment) {
        use_last_arc_count++;
        if (use_last_arc_count >= 3) {
            ROS_ERROR("连续 %d 次使用上一次的有效轮廓! 原因: %s", use_last_arc_count, errorReason.c_str());
        } else {
            ROS_WARN("使用上一次的有效轮廓 (第 %d 次), 原因: %s", use_last_arc_count, errorReason.c_str());
        }
        return last_valid_arc;
    }
    
    // 如果没有上一次有效轮廓或已经成功检测到新轮廓，返回resultArc
    return resultArc;
}

// 在图像上绘制圆弧 - 使用已拟合的椭圆参数
void PointCloudDetect::drawArc(cv::Mat& image, const ArcSegment& arc, const cv::Scalar& color, int thickness) {
    if (arc.points.empty()) {
        ROS_WARN("圆弧没有点，无法绘制");
        return;
    }
    
    // 在右上角显示总帧数
    std::string frameInfo = "total count:" + std::to_string(frame_count);
    
    // 创建背景 - 使用常量定义大小和位置
    cv::Rect frameBgRect(image.cols - FRAME_INFO_RIGHT, FRAME_INFO_TOP, FRAME_INFO_WIDTH, FRAME_INFO_HEIGHT);
    cv::rectangle(image, frameBgRect, cv::Scalar(0, 0, 0, 128), -1);
    
    // 显示文本
    cv::putText(image, frameInfo, cv::Point(image.cols - FRAME_INFO_WIDTH + 10, 30), 
                cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 255, 255), 1);
    
    // 绘制轮廓点
    for (const auto& point : arc.points) {
        cv::circle(image, point, 1, cv::Scalar(0, 255, 0), -1);
    }
    
    // 计算轮廓长度
    double contourLength = cv::arcLength(arc.points, false);
    
    // 计算角度范围
    float startAngle = arc.startAngle * 180.0f / M_PI;
    float endAngle = arc.endAngle * 180.0f / M_PI;
    float angleRange = endAngle - startAngle;
    if (angleRange < 0) angleRange += 360.0f;
    
    // 绘制椭圆弧
    cv::ellipse(image, arc.ellipse.center, 
               cv::Size(arc.ellipse.size.width/2, arc.ellipse.size.height/2), 
               arc.ellipse.angle, startAngle, endAngle, color, thickness);
    
    // 绘制椭圆中心 - 使用常量
    cv::circle(image, arc.ellipse.center, CENTER_POINT_RADIUS, cv::Scalar(0, 0, 255), -1);
    cv::circle(image, arc.ellipse.center, CENTER_POINT_BORDER, cv::Scalar(255, 255, 255), 1);
    
    // 显示中心点坐标，使用红色字体
    std::string centerCoord = "(" + std::to_string(int(arc.ellipse.center.x)) + 
                            ", " + std::to_string(int(arc.ellipse.center.y)) + ")";
    cv::Point coordPos(arc.ellipse.center.x - 100, arc.ellipse.center.y + 30);
    // cv::Rect coordBgRect(coordPos.x - 5, coordPos.y - 20, COORD_BOX_WIDTH, COORD_BOX_HEIGHT);
    // cv::rectangle(image, coordBgRect, cv::Scalar(0, 0, 0, 128), -1);
    cv::putText(image, centerCoord, coordPos, cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, cv::Scalar(0, 0, 255), 1);
    
    // 计算椭圆的平均半径
    float avgRadius = (arc.ellipse.size.width + arc.ellipse.size.height) / 4.0f;
    
    // 显示椭圆信息
    std::string radiusInfo = "R=" + std::to_string(int(avgRadius)) + "px";
    std::string confInfo = "C=" + std::to_string(arc.confidence).substr(0, 4);
    std::string angleInfo = "A=" + std::to_string(int(angleRange)) + "°";
    std::string lengthInfo = "L=" + std::to_string(int(contourLength)) + "px";
    
    // 显示评分信息
    std::string radiusScoreInfo = "Rs=" + std::to_string(arc.radiusScore).substr(0, 4);
    std::string angleScoreInfo = "As=" + std::to_string(arc.angleScore).substr(0, 4);
    std::string radialScoreInfo = "Ds=" + std::to_string(arc.radialScore).substr(0, 4);
    
    // 计算并显示弧度值对应的角度
    int minDegrees = int(MIN_ARC_ANGLE * 180.0f / M_PI);
    int maxDegrees = int(MAX_ARC_ANGLE * 180.0f / M_PI);
    std::string rangeInfo = "(" + std::to_string(minDegrees) + "-" + std::to_string(maxDegrees) + "°)";
    
    // 显示信息（调整为更加美观的布局）- 使用常量定义大小
    cv::Point infoPos(arc.ellipse.center.x + 40, arc.ellipse.center.y - 40);
    // cv::Rect bgRect(infoPos.x - 5, infoPos.y - 15, INFO_BOX_WIDTH, INFO_BOX_HEIGHT);
    // cv::rectangle(image, bgRect, cv::Scalar(0, 0, 0, 128), -1);
    
    // 绘制信息文本 - 使用常量定义行高
    int yOffset = infoPos.y;
    cv::putText(image, radiusInfo, cv::Point(infoPos.x, yOffset), cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, color, 1);
    yOffset += TEXT_LINE_HEIGHT;
    cv::putText(image, confInfo, cv::Point(infoPos.x, yOffset), cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, color, 1);
    yOffset += TEXT_LINE_HEIGHT;
    cv::putText(image, angleInfo, cv::Point(infoPos.x, yOffset), cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, color, 1);
    yOffset += TEXT_LINE_HEIGHT;
    cv::putText(image, lengthInfo, cv::Point(infoPos.x, yOffset), cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, color, 1);
    yOffset += TEXT_LINE_HEIGHT;
    
    // 绘制各项评分
    cv::putText(image, radiusScoreInfo, cv::Point(infoPos.x, yOffset), cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, cv::Scalar(255, 128, 0), 1);
    yOffset += TEXT_LINE_HEIGHT;
    cv::putText(image, angleScoreInfo, cv::Point(infoPos.x, yOffset), cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, cv::Scalar(0, 255, 128), 1);
    yOffset += TEXT_LINE_HEIGHT;
    cv::putText(image, radialScoreInfo, cv::Point(infoPos.x, yOffset), cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, cv::Scalar(128, 0, 255), 1);
    yOffset += TEXT_LINE_HEIGHT;
    
    cv::putText(image, rangeInfo, cv::Point(infoPos.x, yOffset), cv::FONT_HERSHEY_SIMPLEX, TEXT_INFO_SCALE, cv::Scalar(128, 128, 128), 1);
    
    // 绘制起始点和结束点
    float rad1 = startAngle * M_PI / 180.0f;
    float rad2 = endAngle * M_PI / 180.0f;
    
    // 旋转角度需要考虑椭圆的旋转
    float rotAngle = arc.ellipse.angle * M_PI / 180.0f;
    
    // 计算椭圆上的点
    float a = arc.ellipse.size.width / 2.0f;
    float b = arc.ellipse.size.height / 2.0f;
    
    // 使用辅助函数计算椭圆上的点
    cv::Point2f startPoint = calculateEllipsePoint(arc.ellipse.center, a, b, rad1, rotAngle);
    cv::Point2f endPoint = calculateEllipsePoint(arc.ellipse.center, a, b, rad2, rotAngle);
    
    // 绘制半透明扇形区域标识角度范围
    cv::Mat overlay;
    image.copyTo(overlay);
    
    std::vector<cv::Point> arcPoints;
    arcPoints.push_back(arc.ellipse.center);
    

    // 在中心附近绘制角度标签
    float middleAngle = (startAngle + endAngle) / 2.0f;
    if (middleAngle >= 360.0f) middleAngle -= 360.0f;
    float middleRad = middleAngle * M_PI / 180.0f;
    
    // 使用常量定义标签距离比例
    float labelDist = avgRadius * LABEL_DISTANCE_RATIO;
    // 使用辅助函数计算标签位置
    cv::Point2f labelPoint = calculateEllipsePoint(arc.ellipse.center, labelDist, labelDist, middleRad, rotAngle);
    
    std::string shortAngleInfo = std::to_string(int(angleRange)) + "'";
    cv::putText(image, shortAngleInfo, 
              cv::Point(labelPoint.x - 10, labelPoint.y + 5), 
              cv::FONT_HERSHEY_SIMPLEX, TEXT_SCALE, cv::Scalar(255, 255, 255), 1);
}



// 调试图像显示函数
void PointCloudDetect::showDebugImage(const cv::Mat& image, const std::string& window_name) {
    if (debug_mode) {
        // 创建或获取命名窗口
        cv::namedWindow(window_name, cv::WINDOW_NORMAL);
        
        // 设置窗口大小为统一尺寸 - 不要太大
        cv::resizeWindow(window_name, 640, 480);
        
        // // 根据窗口名称设置不同的位置，实现平铺效果
        // if (window_name == "10-最终结果") {
        //     cv::moveWindow(window_name, 520, 50); // 右侧
        // } else if (window_name == "3-细化结果") {
        //     cv::moveWindow(window_name, 10, 50);  // 左侧
        // }
        
        // 显示图像
        cv::imshow(window_name, image);
        cv::waitKey(1); // 更新显示，不阻塞
    }
}

void PointCloudDetect::refineContourPoints(std::vector<std::vector<cv::Point>>& contours) {
    bool merged;
    do {
        merged = false;
        for (size_t i = 0; i < contours.size(); ++i) {
            if (contours[i].empty()) continue;
            
            cv::Point i_front = contours[i].front();
            cv::Point i_back = contours[i].back();

            for (size_t j = i+1; j < contours.size(); ++j) {
                if (contours[j].empty()) continue;
                
                cv::Point j_front = contours[j].front();
                cv::Point j_back = contours[j].back();

                // 计算四点间最小距离[6](@ref)
                double min_dist = DBL_MAX;
                std::pair<cv::Point, cv::Point> closest_pair;
                
                // 定义距离计算和配对逻辑
                auto update_min = [&](cv::Point a, cv::Point b) {
                    double dist = cv::norm(a - b);
                    if (dist < min_dist) {
                        min_dist = dist;
                        closest_pair = {a, b};
                    }
                };
                
                update_min(i_front, j_front);
                update_min(i_front, j_back);
                update_min(i_back, j_front); 
                update_min(i_back, j_back);

                if (min_dist > 1 && min_dist <= 20) {
                    // 生成插值点序列[5](@ref)
                    std::vector<cv::Point> interpolated;
                    cv::Point start = closest_pair.first;
                    cv::Point end = closest_pair.second;
                    
                    // 计算插值步数（步长1像素）[1](@ref)
                    int steps = static_cast<int>(min_dist) - 1;
                    for (int k = 1; k <= steps; ++k) {
                        // 双线性插值公式[5](@ref)
                        double ratio = static_cast<double>(k) / (steps + 1);
                        interpolated.emplace_back(
                            start.x + ratio * (end.x - start.x),
                            start.y + ratio * (end.y - start.y)
                        );
                    }
                    
                    // 确定插入位置和方向[6](@ref)
                    if (closest_pair.first == i_back) {
                        contours[i].insert(contours[i].end(), interpolated.begin(), interpolated.end());
                        contours[i].insert(contours[i].end(), contours[j].begin(), contours[j].end());
                    } else if (closest_pair.first == i_front) {
                        contours[i].insert(contours[i].begin(), interpolated.rbegin(), interpolated.rend());
                        contours[i].insert(contours[i].begin(), contours[j].rbegin(), contours[j].rend());
                    }
                    
                    contours[j].clear();
                    merged = true;
                    break;
                }
                else if (min_dist <= 1) { // 直接合并
                    // 原始合并逻辑保持不变...
                }
            }
            if (merged) break;
        }
        
        // 清理空轮廓[6](@ref)
        contours.erase(std::remove_if(contours.begin(), contours.end(),
            [](const auto& c) { return c.empty(); }), contours.end());
        
    } while (merged);
}

// 预处理并提取轮廓
cv::Mat PointCloudDetect::preprocessAndExtractContours(const cv::Mat& image, std::vector<std::vector<cv::Point>>& contours) {
    // 步骤1: 预处理图像 - 获取骨架化的二值图像
    // ROS_INFO("步骤1: 图像预处理");
    cv::Mat processed;
    preprocessImage(image, processed);
    
    // 步骤2: 提取轮廓
    // ROS_INFO("步骤2: 提取轮廓");
    contours = extractContours(processed);
    ROS_INFO("总共提取到 %zu 个轮廓", contours.size());
    cv::Mat image_copy = image.clone();
    cv::drawContours(image_copy, contours, -1, cv::Scalar(0, 255, 0), 1); // 绘制所有轮廓
    showDebugImage(image_copy, "2-");

    refineContourPoints(contours);
    ROS_INFO("合并提取到 %zu 个轮廓", contours.size());
    cv::Mat image_copy2 = image.clone();
    cv::drawContours(image_copy2, contours, -1, cv::Scalar(0, 255, 0), 1); // 绘制所有轮廓
    showDebugImage(image_copy2, "3-");

    // 不再显示中间调试图像，保留处理后的图像用于后续处理
    return processed;
}

// 筛选轮廓
std::vector<std::vector<cv::Point>> PointCloudDetect::filterContours(const std::vector<std::vector<cv::Point>>& contours, const cv::Mat& image) {
    // 预先分配空间以避免多次重新分配
    std::vector<std::vector<cv::Point>> filteredContours;
    filteredContours.reserve(contours.size() / 2); // 假设大约一半轮廓会通过筛选
    
    // 筛选轮廓：长度和长宽比
    for (int i = 0; i < contours.size(); i++) {
        const auto& contour = contours[i];
        
        // 快速筛选：如果点数太少，直接跳过
        if (contour.size() < 10) {
            continue;
        }
        
        // 检查轮廓长度
        double length = cv::arcLength(contour, false);
        if (length < MIN_CONTOUR_LENGTH) {
            ROS_INFO("轮廓#%d被跳过: 长度(%.1f) < 最小长度(%d)", i, length, MIN_CONTOUR_LENGTH);
            continue;
        }
        ROS_INFO("轮廓#%d长度: %.1f", i, length);
        // 检查轮廓长宽比
        cv::Rect bounds = cv::boundingRect(contour);
        float aspectRatio = bounds.width > bounds.height ? 
                          (float)bounds.width / bounds.height : 
                          (float)bounds.height / bounds.width;
        
        if (aspectRatio < 0.8f || aspectRatio > 2.3f) {
            ROS_INFO("轮廓#%d被跳过: 长宽比(%.2f)不在1.0-1.5范围内", i, aspectRatio);
            continue;
        }
        ROS_INFO("轮廓#%d长宽比: %.2f", i, aspectRatio);
        
        // 通过筛选
        filteredContours.push_back(contour);
    }
    ROS_INFO("筛选后轮廓数量: %zu", filteredContours.size());
    cv::Mat image_copy = cv::Mat::zeros(image.size(), CV_8UC3);
    
    // 直接使用drawContours绘制轮廓
    for (int i = 0; i < filteredContours.size(); i++) {
        cv::drawContours(image_copy, filteredContours, i, cv::Scalar(0, 0, 255), 2); // 红色轮廓
    }
    showDebugImage(image_copy, "轮廓结果");
    // 不再创建和显示调试图像
    return filteredContours;
}



// 计算圆弧置信度
float PointCloudDetect::calculateArcConfidence(ArcSegment& arc, float pixelRadius, float arcAngle) {
    // 计算各项评分 - 使用辅助函数
    float radiusScore = calculateScoreFromRange(pixelRadius, TARGET_RADIUS, MIN_RADIUS, MAX_RADIUS);
    float angleScore = calculateScoreFromRange(arcAngle, (MIN_ARC_ANGLE + MAX_ARC_ANGLE)/2, MIN_ARC_ANGLE, MAX_ARC_ANGLE);
    float radialDistributionScore = calculateRadialDistribution(arc.points, arc.ellipse.center);
    
    // 保存评分到圆弧结构中，方便后续显示
    arc.radiusScore = radiusScore;
    arc.angleScore = angleScore;
    arc.radialScore = radialDistributionScore;
    
    // 使用定义的权重常量
    float confidence = (radiusScore * RADIUS_SCORE_WEIGHT + 
                      angleScore * ANGLE_SCORE_WEIGHT + 
                      radialDistributionScore * RADIAL_SCORE_WEIGHT);
    
    // 输出调试信息
    // ROS_INFO("圆弧置信度: %.2f (半径=%.2f, 角度=%.2f, 径向分布=%.2f)", 
    //          confidence, radiusScore, angleScore, radialDistributionScore);
    
    return confidence;
}

// 从轮廓中检测圆弧
std::vector<ArcSegment> PointCloudDetect::detectArcsFromContours(const std::vector<std::vector<cv::Point>>& filteredContours, const cv::Mat& image) {
    // 步骤3: 对轮廓进行椭圆拟合，获取半圆轮廓
    // ROS_INFO("步骤3: 椭圆拟合和半圆检测");
    std::vector<ArcSegment> detectedArcs;
    
    // 创建一个复用的图像用于显示圆弧
    cv::Mat arcPreview;
    
    for (int i = 0; i < filteredContours.size(); i++) {
        const auto& contour = filteredContours[i];
        // ROS_INFO("分析轮廓 #%d (点数: %zu)", i, contour.size());
        
        // 检查点数是否足够进行椭圆拟合
        if (contour.size() < 5) {
            // ROS_INFO("轮廓#%d点数不足以进行椭圆拟合 (%zu < 5)", i, contour.size());
            continue;
        }
        
        // 创建圆弧结构
        ArcSegment arc;
        
        // 只在拟合椭圆步骤使用try-catch
        cv::RotatedRect ellipse;
        try {
            // 使用OpenCV椭圆拟合
            ellipse = cv::fitEllipse(contour);
            arc.ellipse = ellipse;  // 先保存椭圆拟合结果
        } catch (const cv::Exception& e) {
            ROS_WARN("轮廓#%d椭圆拟合失败: %s", i, e.what());
            continue;
        }
        
        // 计算半径
        float pixelRadius = calculateRadius(contour, ellipse);
        
        // 计算圆弧角度范围
        float arcAngle;
        try {
            arcAngle = calculateArcAngle(contour, ellipse);
        } catch (const std::exception& e) {
            ROS_WARN("轮廓#%d角度计算失败: %s", i, e.what());
            continue;
        }
        
        // 填充圆弧结构
        // arc.center = ellipse.center;
        arc.radius = pixelRadius;
        arc.points.assign(contour.begin(), contour.end());
        arc.startAngle = 0;
        arc.endAngle = arcAngle;
        
        // 计算置信度
        try {
            arc.confidence = calculateArcConfidence(arc, pixelRadius, arcAngle);
        } catch (const std::exception& e) {
            ROS_WARN("轮廓#%d置信度计算失败: %s", i, e.what());
            continue;
        }
        
        // 如果置信度足够高，保存结果 
        if (arc.confidence > CONFIDENCE_THRESHOLD) {
            detectedArcs.push_back(arc);
        }
    }
    
    // 按置信度排序
    if (!detectedArcs.empty()) {
        std::sort(detectedArcs.begin(), detectedArcs.end(), 
                [](const ArcSegment& a, const ArcSegment& b) {
                    return a.confidence > b.confidence;
                });
    }
    
    return detectedArcs;
}

// 使用低通滤波获取平滑中心点
cv::Point2f PointCloudDetect::getLowPassSmoothCenter(cv::Point2f& cloudPoint) {
    if (isFirstCloudCenter) {
        lastCloudCenter = cloudPoint;
        isFirstCloudCenter = false;
        return cloudPoint;
    }
    
    // 低通滤波公式: y[n] = α * x[n] + (1-α) * y[n-1]
    cv::Point2f smoothCloudPoint;
    smoothCloudPoint.x = ALPHA * cloudPoint.x + (1 - ALPHA) * lastCloudCenter.x;
    smoothCloudPoint.y = ALPHA * cloudPoint.y + (1 - ALPHA) * lastCloudCenter.y;
    
    // 更新上一次平滑结果
    lastCloudCenter = smoothCloudPoint;
    
    // 可以添加调试信息
    ROS_INFO("原始点云坐标: (%.3f, %.3f), 平滑后: (%.3f, %.3f)", 
             cloudPoint.x, cloudPoint.y, smoothCloudPoint.x, smoothCloudPoint.y);
    
    return smoothCloudPoint;
}

// 可视化并发布结果
cv::Mat PointCloudDetect::visualizeAndPublishResults(const ArcSegment& arc, const cv::Mat& resultImage) {
    // 如果没有检测到圆弧，直接返回原图像
    if (arc.points.empty() || arc.confidence <= 0) {
        return resultImage;
    }
    
    // 使用引用直接在原图像上绘制，避免克隆图像
    // 检查输入图像是否可写(不是只读副本)
    cv::Mat finalResult;
    if (resultImage.elemSize() == resultImage.step1() * resultImage.rows) {
        // 图像数据是连续的，可以直接使用
        finalResult = resultImage;
    } else {
        // 仅在必要时创建副本
        resultImage.copyTo(finalResult);
    }
    
    // ROS_INFO("绘制圆弧: 半径=%.2f, 置信度=%.2f", arc.radius, arc.confidence);

    // 绘制最佳圆弧
    drawArc(finalResult, arc, cv::Scalar(0, 0, 255), 2);
    
    // 发布中心点
    PointCloud2D centerCloud;
    cv::Point2f cloudPoint = imageToPointCloud(arc.ellipse.center);
    
    // 对点云坐标进行低通滤波
    cv::Point2f smoothCloudPoint = getLowPassSmoothCenter(cloudPoint);

    Point2D centerPoint;
    centerPoint.x = smoothCloudPoint.x;
    centerPoint.y = smoothCloudPoint.y;
    centerCloud.push_back(centerPoint);
    ROS_INFO("centerPoint: %.2f, %.2f", centerPoint.x, centerPoint.y);
    // 使用通用点云发布函数发布中心点
    PointCloud2DPtr centerCloudPtr = std::make_shared<PointCloud2D>(centerCloud);
    publishPointCloud(centerCloudPtr, cross_pub);
    
    // 显示最终结果
    // showDebugImage(finalResult, "10-最终结果");
    
    return finalResult;
}

// 更新帧计数函数
void PointCloudDetect::updateFrameCount() {
    // 只增加帧计数（总帧数）
    frame_count++;
}



/*
================= 检测直线 函数 =======================
*/

// 筛选轮廓
std::vector<std::vector<cv::Point>> PointCloudDetect::filterContoursLine(const std::vector<std::vector<cv::Point>>& contours, const cv::Mat& image) {
    // 预先分配空间以避免多次重新分配
    std::vector<std::vector<cv::Point>> filteredContours;
    filteredContours.reserve(contours.size() / 2 + 1); // 假设大约一半轮廓会通过筛选
    
    // 筛选轮廓：长度和长宽比
    for (int i = 0; i < contours.size(); i++) {
        const auto& contour = contours[i];
        
        // 快速筛选：如果点数太少，直接跳过
        if (contour.size() < 5) {
            ROS_ERROR("点数太小< 5，跳过轮廓#%d", i);
            continue;
        }
        
        // 检查轮廓长度
        // double length = cv::arcLength(contour, false);
        int length = contour.size();
        if (length > 50) {
            ROS_INFO("轮廓#%d被跳过: 长度(%.1f) > 最大长度(%d)", i, length, 50);
            continue;
        }
        // ROS_INFO("轮廓#%d长度: %.1f", i, length);
        // 检查轮廓长宽比
        cv::Rect bounds = cv::boundingRect(contour);
        float aspectRatio = bounds.width > bounds.height ? 
                          (float)bounds.width / bounds.height : 
                          (float)bounds.height / bounds.width;
        
        if (aspectRatio > 0.5f && aspectRatio < 2.0f) {
            ROS_INFO("轮廓#%d被跳过: 长宽比(%.2f)在0.5-2.0范围内", i, aspectRatio);
            continue;
        }
        ROS_INFO("轮廓#%d长宽比: %.2f", i, aspectRatio);
        
        // 通过筛选
        filteredContours.push_back(contour);
    }
    ROS_INFO("筛选后轮廓数量: %zu", filteredContours.size());
    // 不再创建和显示调试图像
    return filteredContours;
}

    // 公共直线拟合计算
void PointCloudDetect::fitLineCommon(const std::vector<cv::Point>& points, 
                            cv::Vec4f& lineParams, 
                            cv::Point2f& refPoint,
                            cv::Point2f& dir) {
    cv::fitLine(points, lineParams, cv::DIST_L2, 0, 0.01, 0.01);
    refPoint = cv::Point2f(lineParams[2], lineParams[3]);
    dir = cv::Point2f(lineParams[0], lineParams[1]);
}

// 计算投影范围
void PointCloudDetect::calcProjectionRange(const std::vector<cv::Point>& points,
                                const cv::Point2f& refPoint,
                                const cv::Point2f& dir,
                                float& t_min, float& t_max) {
    t_min = std::numeric_limits<float>::max();
    t_max = -t_min;
    for (const auto& pt : points) {
        float t = (pt.x - refPoint.x) * dir.x + (pt.y - refPoint.y) * dir.y;
        t_min = std::min(t_min, t);
        t_max = std::max(t_max, t);
    }
}
// 合并后的直线拟合与评估函数
LineSegment PointCloudDetect::evaluateLine(const std::vector<cv::Point>& points, cv::Mat& image) {

    // 公共拟合计算
    cv::Vec4f lineParams;
    cv::Point2f refPoint, dir;
    LineSegment lineSegment;
    ROS_INFO("开始拟合直线，点数: %zu", points.size());

    fitLineCommon(points, lineParams, refPoint, dir);

    // 1. 残差分析
    float sse = std::accumulate(points.begin(), points.end(), 0.0f, 
        [&](float sum, const cv::Point& pt) {
            float num = std::abs(dir.x*(pt.y-refPoint.y) - dir.y*(pt.x-refPoint.x));
            return sum + (num*num)/(dir.x*dir.x + dir.y*dir.y);
        });
    const float sse_score = 1.0f / (1.0f + sse/points.size());

    // 三. 使用点的分布均匀性分析
    std::vector<float> distances; // 存储所有点到直线的距离
    float total_length = 0; // 计算点集的总长度
    
    // 计算所有点到直线的距离和点集总长度
    cv::Point2f last_pt;
    bool first = true;
    
    for (const auto& pt : points) {
        // 计算点到直线的距离
        float dist = std::abs(dir.x*(pt.y-refPoint.y) - dir.y*(pt.x-refPoint.x)) / 
                    std::sqrt(dir.x*dir.x + dir.y*dir.y);
        distances.push_back(dist);
        
        // 计算相邻点之间的距离
        if (!first) {
            total_length += cv::norm(cv::Point2f(pt) - last_pt);
        }
        last_pt = pt;
        first = false;
    }
    
    // 计算距离的统计特征
    float mean_dist = std::accumulate(distances.begin(), distances.end(), 0.0f) / distances.size();
    float variance = 0.0f;
    for (float d : distances) {
        variance += (d - mean_dist) * (d - mean_dist);
    }
    variance /= distances.size();
    
    // 计算点的分布均匀性得分
    float avg_segment = total_length / (points.size() - 1);
    float distribution_score = std::exp(-variance/avg_segment);

    // 综合评分与绘制
    const float total_score = 0.5*sse_score  + 0.5*distribution_score;

    lineSegment.sseScore = sse_score;
    lineSegment.distScore = distribution_score;
    lineSegment.totalScore = total_score;
    lineSegment.middlePoint = getLineMiddlePoint(lineParams);
    lineSegment.length = points.size();

    ROS_WARN("SSE: %.2f, Dis: %.2f, Total: %.2f,(%.2f, %.2f) ", 
            sse_score, distribution_score, total_score, lineSegment.middlePoint.x, lineSegment.middlePoint.y);
    drawResults(image, points, lineParams, sse_score, distribution_score, total_score);
    return lineSegment;
}

cv::Point2f PointCloudDetect::getLineMiddlePoint(const cv::Vec4f& lineParams) {
    // lineParams的格式：(vx, vy, x0, y0)
    // (vx, vy) 是方向向量
    // (x0, y0) 是直线上的一个点
    
    // 获取参考点和方向向量
    cv::Point2f refPoint(lineParams[2], lineParams[3]);
    cv::Point2f dir(lineParams[0], lineParams[1]);
    
    // 在参考点两侧各取一定距离
    float offset = 100.0f;  // 可以根据实际需求调整这个值
    cv::Point2f pt1 = refPoint - offset * dir;
    cv::Point2f pt2 = refPoint + offset * dir;
    
    // 计算中点
    cv::Point2f midPoint((pt1.x + pt2.x) / 2.0f, 
                        (pt1.y + pt2.y) / 2.0f);
    
    return midPoint;
}

// 统一绘制函数
void PointCloudDetect::drawResults(cv::Mat& image, 
                        const std::vector<cv::Point>& points,
                        const cv::Vec4f& lineParams,
                        float sse, float dist, float total) {
    // 绘制拟合直线
    float t_min, t_max;
    cv::Point2f refPoint(lineParams[2], lineParams[3]), dir(lineParams[0], lineParams[1]);
    calcProjectionRange(points, refPoint, dir, t_min, t_max);
    cv::Point2f pt1 = refPoint + t_min * dir;
    cv::Point2f pt2 = refPoint + t_max * dir;
    cv::line(image, pt1, pt2, cv::Scalar(0,255,0), 2);
    // 计算中点
    cv::Point2f midPoint((pt1.x + pt2.x) / 2.0f, 
                        (pt1.y + pt2.y) / 2.0f);
    cv::circle(image, midPoint, 2, cv::Scalar(255,0,0), -1);
    // 绘制点集与评估结果
    // for (const auto& pt : points) cv::circle(image, pt, 3, cv::Scalar(0,0,255), -1);
    
    // 分行显示评分（自动避让）
    cv::Point text_origin(refPoint.x + ((std::abs(lineParams[1]) > 0.5) ? -30 : 30), 
                            refPoint.y - 10);
    std::vector<std::string> lines = {
        cv::format("SSE: %.2f", sse),
        cv::format("Dist: %.2f", dist),
        cv::format("Total: %.2f", total),
        cv::format("len: %zu", points.size())
    };
    for (size_t i = 0; i < lines.size(); ++i) {
        cv::putText(image, lines[i], 
            cv::Point(text_origin.x+20, text_origin.y + i*8),
            cv::FONT_HERSHEY_SIMPLEX, 0.3, 
            (total > 0.6) ? cv::Scalar(0,0,200) : cv::Scalar(0,160,0), 0.2);
    }
}

float PointCloudDetect::calculateDistance(const cv::Point2f& p1, const cv::Point2f& p2) {
    float dx = p1.x - p2.x;
    float dy = p1.y - p2.y;
    return std::sqrt(dx * dx + dy * dy);
}

LineSegment PointCloudDetect::processImageLine(const cv::Mat& image)
{
    // 创建一个空的ArcSegment用于返回
    LineSegment lineSegment;
    lineSegment.totalScore = 0.0f; // 初始化总评分为0，表示未找到有效轮廓
    std::vector<std::vector<cv::Point>> filteredContours;
    // 1. 检查是否需要使用上一次有效轮廓
    bool useLastSegment = false;
    std::string errorReason = "";
    
    // 输入图像为空
    if (image.empty()) {
        errorReason = "输入图像为空";
        lineSegment = last_valid_line; // 使用上次有效轮廓
        use_last_arc_count++;
        return lineSegment;
    }
    // 创建结果图像（彩色），仅在需要时复制
    cv::Mat result;
    if (image.channels() == 3) {
        // 如果已经是彩色图像，可以直接使用
        image.copyTo(result);
    } else {
        // 如果是灰度图像，转换为彩色
        cv::cvtColor(image, result, cv::COLOR_GRAY2BGR);
    }
    
    // 2. 预处理图像并提取轮廓
    std::vector<std::vector<cv::Point>> allContours;
    cv::Mat processed = preprocessAndExtractContours(image, allContours);
    
    // 检查轮廓数量
    if (allContours.empty()) {
        errorReason = "未检测到任何轮廓";
        lineSegment = last_valid_line; // 使用上次有效轮廓
        use_last_arc_count++;
        return lineSegment;
    }
    cv::Mat image_copy = image.clone();
    drawContours(image_copy, allContours, -1, cv::Scalar(0, 0, 255), 1);
    showDebugImage(image_copy, "2-预处理结果");
    // 3. 如果有轮廓，继续筛选
    filteredContours = filterContoursLine(allContours, image);
    
    // 检查筛选后的轮廓数量
    if (filteredContours.empty()) {
        errorReason = "没有符合条件的轮廓";
        lineSegment = last_valid_line; // 使用上次有效轮廓
        use_last_arc_count++;
        return lineSegment;
    }

    // 4. 如果有符合条件的轮廓，进行直线拟合
    float min_distance_to_origin = std::numeric_limits<float>::max();
    LineSegment best_line_segment;
    best_line_segment.totalScore = 0;

    // 计算图像坐标系中的原点位置
    cv::Point2f origin_point(
        static_cast<float>(static_cast<int>(0 - min_x) + this->border),
        static_cast<float>(static_cast<int>(0 - min_y) + this->border)
    );

    // 对每个轮廓进行直线拟合
    for(const auto& contour: filteredContours) {
        LineSegment temp_line = evaluateLine(contour, result);
        
        // 计算中点到原点的距离
        float distance_to_origin = cv::norm(temp_line.middlePoint - origin_point);
        
        // 使用评分和距离的组合来选择最佳线段
        // 如果评分相近（差异小于0.1），选择距离更近的
        if (temp_line.totalScore >= 0.7)
        {
            if (temp_line.totalScore > best_line_segment.totalScore + 0.1 || 
                (std::abs(temp_line.totalScore - best_line_segment.totalScore) <= 0.1 && 
                distance_to_origin < min_distance_to_origin)) {

                best_line_segment = temp_line;
                min_distance_to_origin = distance_to_origin;
            }
        }
    }

    // 使用选定的最佳线段
    if (best_line_segment.totalScore > 0) {
        // 对于突变值应该采用上一次有效值
        lineSegment = best_line_segment;
        if (calculateDistance(lineSegment.middlePoint, last_valid_line.middlePoint) > 20 &&
            last_valid_line.totalScore > 0) {
            if(use_last_arc_count < 3) {
                ROS_INFO("now:(%.2f,%.2f), last:(%.2f,%.2f)", 
                    lineSegment.middlePoint.x, lineSegment.middlePoint.y,
                    last_valid_line.middlePoint.x, last_valid_line.middlePoint.y);
                lineSegment = last_valid_line;
                use_last_arc_count++;
                ROS_WARN("当前轮廓与上次轮廓差异过大，使用上次轮廓");
            } else{
                ROS_ERROR("更新中点");
                use_last_arc_count = 0;
            }
        }else{
            use_last_arc_count = 0; 
        }

        // 输出调试信息
        cv::Point2f middle_point = imageToPointCloud(lineSegment.middlePoint);
        publishCenterPoint(middle_point, cross_pub);

        ROS_ERROR("中点位置 - 点云坐标: (%.2f, %.2f)", 
                middle_point.x, middle_point.y);
    }else{
        // 如果没有找到有效的直线，使用上次有效轮廓
        lineSegment = last_valid_line;
        use_last_arc_count++;
        ROS_WARN("未找到有效直线，使用上次有效轮廓");
        return lineSegment;
    }
    showDebugImage(result, "3-直线拟合结果");
    last_valid_line = lineSegment;
   
    // 如果没有上一次有效轮廓或已经成功检测到新轮廓，返回resultArc
    return lineSegment;
}

void PointCloudDetect::publishCenterPoint(cv::Point2f& centerPoint, ros::Publisher& pub)
{
    PointCloud2D centerCloud;
    Point2D targetPoint;
    targetPoint.x = centerPoint.x;
    targetPoint.y = centerPoint.y;
    centerCloud.push_back(targetPoint);
    ROS_INFO("targetPoint: %.2f, %.2f", targetPoint.x, targetPoint.y);
    // 使用通用点云发布函数发布中心点
    PointCloud2DPtr centerCloudPtr = std::make_shared<PointCloud2D>(centerCloud);
    publishPointCloud(centerCloudPtr, pub);
}


void PointCloudDetect::pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    // 更新帧率
    updateFrameCount();
        
    // 创建新的点云消息的共享指针
    auto modified_cloud_msg = std::make_shared<sensor_msgs::PointCloud2>(*cloud_msg);
        
    // 创建临时向量存储有效点的索引
    std::vector<size_t> valid_indices;
    valid_indices.reserve(modified_cloud_msg->width);
    
    // 遍历所有点，找出距离小于1.5米的点
    const float MAX_DISTANCE = 1.0f; // 最大距离阈值（米）
    const float MAX_DISTANCE_SQ = MAX_DISTANCE * MAX_DISTANCE; // 平方后的距离阈值
    const float MIN_DISTANCE_SQ = 0.3f * 0.3f; // 最小距离阈值（米）

    for (size_t i = 0 ; i < cloud_end_idx * modified_cloud_msg->point_step; i += modified_cloud_msg->point_step) {
        // 获取x、y坐标
        float* x_ptr = reinterpret_cast<float*>(&modified_cloud_msg->data[i]);
        float* y_ptr = reinterpret_cast<float*>(&modified_cloud_msg->data[i + 4]);
        float* z_ptr = reinterpret_cast<float*>(&modified_cloud_msg->data[i + 8]);
        
        // 计算到原点的距离平方
        float distance_sq = (*x_ptr) * (*x_ptr) + (*y_ptr) * (*y_ptr);
        
        // 如果距离在阈值内，记录该点的索引
        if (distance_sq <= MAX_DISTANCE_SQ && distance_sq >= MIN_DISTANCE_SQ) {
            valid_indices.push_back(i);
            *z_ptr = 0.0f; // 将z坐标设为0
        }
    }
    
    // 创建新的点云消息，只包含有效点
    auto range_cloud_msg = std::make_shared<sensor_msgs::PointCloud2>();
    range_cloud_msg->header = modified_cloud_msg->header;
    range_cloud_msg->height = 1;
    range_cloud_msg->width = valid_indices.size();
    range_cloud_msg->fields = modified_cloud_msg->fields;
    range_cloud_msg->point_step = modified_cloud_msg->point_step;
    range_cloud_msg->row_step = range_cloud_msg->point_step * valid_indices.size();
    range_cloud_msg->data.resize(range_cloud_msg->row_step);
    
    // 复制有效点的数据
    for (size_t i = 0; i < valid_indices.size(); ++i) {
        std::memcpy(
            &range_cloud_msg->data[i * range_cloud_msg->point_step],
            &modified_cloud_msg->data[valid_indices[i]],
            range_cloud_msg->point_step
        );
    }
    
    // 发布原始点云和范围点云
    origincloud_pub.publish(*modified_cloud_msg);
    halfcloud_pub.publish(*range_cloud_msg);
    
    // 将ROS消息转换为PointCloud2DPtr
    PointCloud2DPtr cloud_2d = createPointCloud2D();
    rosMsgToPointCloud2D(*range_cloud_msg, *cloud_2d);

    // 将PointCloud2DPtr转换为图像
    cv::Mat image = pointCloud2DToImage(cloud_2d);
    // showDebugImage(image, "2-点云图像");
    // 处理图像并获取检测结果
    LineSegment detectedArc = processImageLine(image);

    detectedArc.middlePoint = getLowPassSmoothCenter(detectedArc.middlePoint);

    ROS_INFO("LineSegment:(%f,%f)", detectedArc.middlePoint.x, detectedArc.middlePoint.y);

}

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pcl_point_cloud_reader");
    PointCloudDetect pcl_detect;

    // 设置点云范围示例 (前100个点)
    pcl_detect.setCloudRange(0, 100);

    // 打开rosbag文件
    rosbag::Bag bag;
    // bag.open("/home/zhangjj/dm_ws/src/function_test/bag/laser_scan.bag", rosbag::bagmode::Read);
    bag.open("/home/zhangjj/dm_ws/src/function_test/bag/scan_matched6.bag", rosbag::bagmode::Read);
    // 创建一个视图来读取bag文件中的消息
    rosbag::View view(bag, rosbag::TopicQuery("/scan_matched_points2"));
    
    // 创建并设置窗口
    // cv::namedWindow("10-最终结果", cv::WINDOW_NORMAL);
    // cv::namedWindow("3-细化结果", cv::WINDOW_NORMAL);
    // cv::resizeWindow("10-最终结果", 500, 400);
    // cv::resizeWindow("3-细化结果", 500, 400);
    // cv::moveWindow("10-最终结果", 520, 50);
    // cv::moveWindow("3-细化结果", 10, 50);
    
    // 播放bag文件中的消息
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
        sensor_msgs::PointCloud2ConstPtr cloud_msg = m.instantiate<sensor_msgs::PointCloud2>();
        if (cloud_msg != NULL)
        {
            pcl_detect.pointCloudCallback(cloud_msg);
        }
        ros::Duration(0.1).sleep();
        // 检查是否有退出信号
        if (ros::isShuttingDown()) {
            ROS_INFO("收到退出信号，程序即将关闭。");
            break; // 退出for循环
        }
        
        // 等待按键
        char key = cv::waitKey(10);
        if (key == 'q' || key == 27) { // 'q' 或 ESC键退出
            ROS_INFO("用户按下退出键，程序即将关闭。");
            break;
        }
    }

    // 关闭bag文件
    bag.close();

    // 关闭OpenCV窗口
    cv::destroyAllWindows();

    return 0;
}